﻿using System;
using System.Diagnostics.Contracts;
using MathService.DSP.Signals;
using MathService.Values;

namespace MathService.DSP
{
    /// <summary>Цифровой фильтр с бесконечной импульсной характеристикой</summary>
    public class CBIH : DigitalFilter, IDigitalFilter, IValueRead<double>, IInitializable<double[], double[]>
    {
        public class PQ
        {
            public Polynom P { get; set; }
            public Polynom Q { get; set; }

            public double[] A { get { return Q.Coefficients; } }
            public double[] B { get { return P.Coefficients; } }

            public PQ() { }
            public PQ(Polynom P, Polynom Q)
            {
                this.P = P;
                this.Q = Q;
            }

            public PQ P2Z_BilinearTransform(double t)
            {
                var pz0 = new Polynom(1, -1);
                var qz0 = new Polynom(1, 1);

                t = 2 / t;
                var Pp = P.ScalePolynom(t);
                var NP = Pp.Power;
                var ppz = new Polynom(1);
                var pqz = new Polynom(1);
                var PPz_array = new Polynom[NP + 1];
                var PQz_array = new Polynom[NP + 1];
                for(var i = 0; i <= NP; i++)
                {
                    PPz_array[i] = ppz;
                    ppz *= pz0;
                    PQz_array[NP - i] = pqz;
                    pqz *= qz0;
                }
                var Pz = new Polynom(0);
                var PQz = PQz_array[0];
                for(var i = 0; i <= NP; i++)
                    Pz += Pp[i] * PPz_array[i] * PQz_array[i];

                var Qp = Q.ScalePolynom(t);
                var NQ = Qp.Power;
                var qpz = new Polynom(1);
                var qqz = new Polynom(1);
                var QPz_array = new Polynom[NQ + 1];
                var QQz_array = new Polynom[NQ + 1];
                for(var i = 0; i <= NQ; i++)
                {
                    QPz_array[i] = qpz;
                    qpz *= pz0;
                    QQz_array[NQ - i] = qqz;
                    qqz *= qz0;
                }
                var Qz = new Polynom(0);
                var QQz = QQz_array[0];
                for(var i = 0; i <= NQ; i++)
                    Qz += Qp[i] * QPz_array[i] * QQz_array[i];

                Pz *= QQz;
                Qz *= PQz;

                var a0 = Qz[0];

                Pz /= a0;
                Qz /= a0;

                return new PQ(Pz, Qz);
            }

            public PQ Z2P_BilinearTransform(double t)
            {
                var pz0 = new Polynom(2, -t);
                var qz0 = new Polynom(2, t);

                var Pp = P;
                var NP = Pp.Power;
                var ppz = new Polynom(1);
                var pqz = new Polynom(1);
                var PPz_array = new Polynom[NP + 1];
                var PQz_array = new Polynom[NP + 1];
                for(var i = 0; i <= NP; i++)
                {
                    PPz_array[i] = ppz;
                    ppz *= pz0;
                    PQz_array[NP - i] = pqz;
                    pqz *= qz0;
                }
                var Pz = new Polynom(0);
                var PQz = PQz_array[0];
                for(var i = 0; i <= NP; i++)
                    Pz += Pp[i] * PPz_array[i] * PQz_array[i];

                var Qp = Q;
                var NQ = Qp.Power;
                var qpz = new Polynom(1);
                var qqz = new Polynom(1);
                var QPz_array = new Polynom[NQ + 1];
                var QQz_array = new Polynom[NQ + 1];
                for(var i = 0; i <= NQ; i++)
                {
                    QPz_array[i] = qpz;
                    qpz *= pz0;
                    QQz_array[NQ - i] = qqz;
                    qqz *= qz0;
                }
                var Qz = new Polynom(0);
                var QQz = QQz_array[0];
                for(var i = 0; i <= NQ; i++)
                    Qz += Qp[i] * QPz_array[i] * QQz_array[i];

                Pz *= QQz;
                Qz *= PQz;

                var a0 = Qz[0];

                Pz /= a0;
                Qz /= a0;

                return new PQ(Pz, Qz);
            }

            public double Value(double x) { return P.Value(x) / Q.Value(x); }
            public Complex Value(Complex z) { return P.Value(z) / Q.Value(z); }
        }

        /// <summary>Коэффициенты прямого фильтра</summary>
        private double[] f_a;
        /// <summary>Коэффициенты реверсивного фильтра</summary>
        private double[] f_b;

        /// <summary>Текущее щначение на выходе фильтра</summary>
        public double Value
        {
            get
            {
                var lv_Summ = .0;
                var i_Length = Order;
                for(var i = 0; i < i_Length; i++)
                    lv_Summ += f_DelayLine[i] * f_a[i];
                return lv_Summ;
            }
        }

        /// <summary>Фильтр с бесконечной импульсной характеристикой</summary>
        /// <param name="a">Массив коэффициентов прямого фильтра</param>
        /// <param name="b">Массив коэффициентов реверсивного фильтра</param>
        public CBIH(double[] a, double[] b)
            : base(Math.Max(a == null ? 0 : a.Length, b == null ? 0 : b.Length))
        {
            Initialize(a, b);
        }

        /// <summary>Иницифлизация начального состояния фильтра с новыми коэффициентами</summary>
        /// <param name="a">Массив коэффициентов прямого фильтра</param>
        /// <param name="b">Массив коэффициентов реверсивного фильтра</param>
        public void Initialize(double[] a, double[] b)
        {
            if(Equals(a[0], 0))
                throw new ArgumentOutOfRangeException(nameof(a), "Параметр a[0] не доллжен быть равен 0");

            var lv_Order = Math.Max(a.Length, b.Length);
            if(lv_Order != Order)
                Initialize(lv_Order);
            else
                Initialize();

            f_a = new double[lv_Order];
            f_b = new double[lv_Order];
            Array.Copy(a, f_a, a.Length);
            Array.Copy(b, f_b, b.Length);


            if(Equals(f_a[0], 1)) return;
            var lv_a = a[0];
            f_a[0] /= lv_a;
            for(var i = 1; i < lv_Order; i++)
            {
                f_a[i] /= lv_a;
                f_b[i] /= lv_a;
            }
        }

        public override DigitalSignal ProcessSignal(DigitalSignal s)
        {
            var x = s.Samples;
            var N = x.Length;
            var y = new double[N];
            var line = new DelayLine(f_DelayLine.Length);
            var a = f_a;
            var b = f_b;
            var filter = a != null && b != null
                ? (xx => Process_BIH(xx, a, b, line))
                : (b == null
                    ? (Func<double, double>)(xx => Process_Recurs(xx, a, line))
                    : (xx => Process_KIH(xx, b, line)));
            for(var i = 0; i < N; i++)
                y[i] = filter(x[i]);
            return new DigitalSignal(s.dx, s.x0, y);
        }

        /// <summary>Обработка единичного отсчёта сигнала</summary>
        /// <param name="x_n">Единичный отсчёт входного воздействия</param>
        /// <returns>Отклик фильтра на входное воздействие</returns>
        public override double Process(double x_n)
        {
            return Process(x_n, f_a, f_b, f_DelayLine);
        }

        protected static double Process(double x_n, double[] a, double[] b, DelayLine Line)
        {
            Contract.Requires(a != null, "Отсутствует массив коэффициентов полинома числителя");
            Contract.Requires(a.Length > 0, "Степень полинома а должна быть больше 0");

            return a != null && b != null ? Process_BIH(x_n, a, b, Line)
                    : (b == null ? Process_Recurs(x_n, a, Line) : Process_KIH(x_n, b, Line));
        }

        /// <summary>Обработка входного воздействия в КИХ режиме</summary>
        /// <param name="x_n">Единичное входное воздействие</param>
        /// <param name="b">Массив коэффициентов прямого фильтра</param>
        /// <param name="Line">Линия задержки фильтра</param>
        /// <returns>Отклик фильтра</returns>
        private static double Process_KIH(double x_n, double[] b, DelayLine Line)
        {
            var lv_KihSumm = .0;
            Line.AddFirst(x_n);
            var i_Length = Line.Length;
            for(var i = 0; i < i_Length; i++)
                lv_KihSumm += Line[i] * b[i];
            return lv_KihSumm;
        }

        /// <summary>Обработка рекурсивным фильтром</summary>
        /// <param name="x_n">Единичный отсчёт входного воздействия</param>
        /// <param name="a">Коэффициенты рекурсивного фильтра</param>
        /// <param name="Line">Линия задержки фильтра</param>
        /// <returns></returns>
        private static double Process_Recurs(double x_n, double[] a, DelayLine Line)
        {
            var lv_RecursSumm = x_n;
            var i_Length = Line.Length;
            for(var i = 1; i < i_Length; i++)
                lv_RecursSumm -= Line[i - 1] * a[i];
            Line.AddFirst(lv_RecursSumm);
            return lv_RecursSumm;
        }

        /// <summary>Обработка входного воздействия фильтром с бесконечной импульсной эарактеристикой</summary>
        /// <param name="x_n">Единичный отсчёт входного водействия</param>
        /// <param name="a">Масив коэффициентов прямого фильтра</param>
        /// <param name="b">Массив коэффициентов рекурсивного фильтра</param>
        /// <param name="Line">Линия задержки фильтра</param>
        /// <returns>Отклик фильтра</returns>
        private static double Process_BIH(double x_n, double[] a, double[] b, DelayLine Line)
        {
            Contract.Requires(a != null && a.Length > 0);
            var lv_KihSumm = .0;
            var lv_RecursSumm = x_n;
            var i_Length = Line.Length;
            for(var i = 1; i < i_Length; i++)
            {
                var z = Line[i - 1];
                lv_KihSumm += z * b[i];
                lv_RecursSumm -= z * a[i];
            }
            Line.AddFirst(lv_RecursSumm);
            lv_KihSumm += lv_RecursSumm * b[0];
            return lv_KihSumm;
        }

        public override Complex TransferFunction(double f, double dt)
        {
            var w = Consts.pi2 * f;
            var p = Complex.Exp(dt, w);
            p = (2 + p) / (2 - p);
            var P = new Polynom(f_b);
            var Q = new Polynom(f_a);
            return P.Value(p) / Q.Value(p);
        }
    }
}